#include <ros/ros.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/CommandTOL.h>
#include <std_msgs/Bool.h>

ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
ros::ServiceClient land_client;

mavros_msgs::State current_state;
bool is_connected = false;

bool is_off_board = false;
bool is_arming = false;
bool is_land = true;

void stateCallBack(const mavros_msgs::State::ConstPtr& msg)
{
    current_state = *msg;
}

void landCallBack(const std_msgs::Bool::ConstPtr& msg)
{
    std::cout << "call for land!" << std::endl;
    if (!is_connected)  return;
    if (msg->data)
    {
        //land
        mavros_msgs::CommandTOL land_cmd;
        land_cmd.request.min_pitch = 0;
        land_cmd.request.yaw = 0.01;
        land_cmd.request.latitude = 0;
        land_cmd.request.longitude = 0;
        land_cmd.request.altitude = 0;	
        land_client.call(land_cmd);
        is_off_board = false;
        is_arming = false;
        is_land = true;
    }
}

void offBoardCallBack(const std_msgs::Bool::ConstPtr& msg)
{
    std::cout << "call for offboard!" << std::endl;
    if (!is_connected)  
    {
        std::cout << "not connected" << std::endl;
        return;
    }
    if (msg->data)
    {
        std::cout << "connected" << std::endl;
        //off_board
        mavros_msgs::SetMode offb_set_mode;
        offb_set_mode.request.custom_mode = "OFFBOARD";
        set_mode_client.call(offb_set_mode);
        is_off_board = true;
    }
}

void armingCallBack(const std_msgs::Bool::ConstPtr& msg)
{
    std::cout << "call for arm!" << std::endl;
    if (!is_connected)  return;
    if (msg->data)
    {
        //unlock
        if (!is_off_board)  return;

        mavros_msgs::CommandBool arm_cmd;
        arm_cmd.request.value = true;
        arming_client.call(arm_cmd);
        is_arming = true;
    }
    else
    {
        //lock
        mavros_msgs::CommandBool arm_cmd;
        arm_cmd.request.value = false;
        arming_client.call(arm_cmd);
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "drone_to_mavros");
    ros::NodeHandle nh("~");

    std::string arming_service;
    std::string set_mode_service;
    std::string land_service;
    int drone_id_ = 0;
    nh.getParam("arming", arming_service);
    nh.getParam("set_mode", set_mode_service);
    nh.getParam("land", land_service);
    nh.getParam("drone_id_", drone_id_);
    std::cout << "service names" << std::endl;
    std::cout << arming_service << std::endl;
    std::cout << set_mode_service << std::endl;
    std::cout << land_service << std::endl;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/uav"+std::to_string(drone_id_)+"/mavros/state", 10, &stateCallBack);
    ros::Subscriber land_sub = nh.subscribe<std_msgs::Bool>("shieh_land", 10, &landCallBack);
    ros::Subscriber offBoard_sub = nh.subscribe<std_msgs::Bool>("shieh_off_board", 10, &offBoardCallBack);
    ros::Subscriber arming_sub = nh.subscribe<std_msgs::Bool>("shieh_arming", 10, &armingCallBack);


    arming_client = nh.serviceClient<mavros_msgs::CommandBool>(arming_service);
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>(set_mode_service);
	land_client = nh.serviceClient<mavros_msgs::CommandTOL>(land_service);

    ros::Rate rate(20.0);

    while(ros::ok() && !current_state.connected)
    {
        is_connected = false;
        ros::spinOnce();
        rate.sleep();
    }

    is_connected = true;
    ROS_WARN("mavros connected");

    while(ros::ok())
    {
        ros::spinOnce();
    }


}